package org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.af;

import org.opentcs.kcvehicle.communication.kc.common.byteutils.ByteUtils;

public class LocationStatusInfo {
  private byte[] src;
  //4个字节,车 全局x,单位 m
  public float globalX;
  //4个字节,车 全局y,单位 m
  public float globalY;
  //机器人绝对车体方向角,单位 rad
  public float absoluteDirecAngle;
  //最后通过点 ID
  public Integer lastPassPointId;
  //最后通过edge ID
  public Integer lastPassEdgeId;
  //最后通过点在任务中的序列号
  public Integer serialNumber;
  //置信度,1个字节
  public byte confidenceLevel;
  //预留8个字节
  public byte[] remain;

  public static void main(String[] args) {
    byte[] src = new byte[32];
    src[0] = (byte) 0x73;
    src[1] = (byte) 0xde;
    src[2] = (byte) 0xba;
    src[3] = (byte) 0xbd;

    src[4] = (byte) 0x34;
    src[5] = (byte) 0xbf;
    src[6] = (byte) 0xd0;
    src[7] = (byte) 0x40;

    src[8] = (byte) 0x59;
    src[9] = (byte) 0x3c;
    src[10] = (byte) 0x1d;
    src[11] = (byte) 0x3d;
    System.out.println("x: " + ByteUtils.bytesToFloat(src, 0));
    System.out.println("y: " + ByteUtils.bytesToFloat(src, 4));
    System.out.println("z: " + ByteUtils.bytesToFloat(src, 8));
  }

  public LocationStatusInfo(byte[] src) {
    this.src = src;
    for (byte b : src) {
      System.out.print(byteToHex(b) + " ");
    }
    //[115, -34, -70, -67, 52, -65, -48, 64, 89, 60, 29, 61, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
    this.globalX = ByteUtils.bytesToFloat(src, 0);
    this.globalY = ByteUtils.bytesToFloat(src, 4);
    this.absoluteDirecAngle = ByteUtils.bytesToFloat(src, 8);
    this.lastPassPointId = ByteUtils.bytesToInt(src, 12);
    this.lastPassEdgeId = ByteUtils.bytesToInt(src, 16);
    this.serialNumber = ByteUtils.bytesToInt(src, 20);
    this.confidenceLevel = src[24];
    this.remain = new byte[7];
    for (int i = 0; i < 7; i++) {
      this.remain[i] = src[25 + i];
    }
  }

  public static String byteToHex(byte b) {
    // 将byte转换为无符号整数
    int unsignedByte = b & 0xFF;
    // 使用Integer.toHexString方法转换为十六进制字符串
    String hexString = Integer.toHexString(unsignedByte);
    // 如果字符串长度为1，需要在前面补0
    if (hexString.length() == 1) {
      return "0" + hexString;
    }
    return hexString;
  }
}
